import math

RADIUS_OF_EARTH = 6371000.0  # 地球半径，单位：米

class AzimuthalProjection:
    """
    等距方位投影 - 将地理坐标转换为平面坐标
            ^  x, North
            |
            | a/
            | /
            |/
    --------------------> y, East
            |
            |
            
    如图所示，
    该投影方法将地球表面上的点映射到一个平面上，
    并保持等距投影的性质。
    该方法的优点是可以保持距离和方向的信息，
    适用于需要计算距离和方位角的应用。其中x轴为正北方向，y轴为正东方向。方位角a单位为度，0-360，正北为0，顺时针增加

    注意：
    该投影方法假设地球是一个完美的球体，
    计算过程中使用了球面余弦定律和反三角函数。
    由于地球的形状，实际计算中可能会存在一些误差。
    该方法适用于小范围内的等距投影，
    对于大范围的投影，可能需要使用其他方法。
    """
    
    @staticmethod
    def project(lat, lon, ref_lat, ref_lon, radius=RADIUS_OF_EARTH):
        """
        计算从参考点到目标点的平面坐标和方位角
        
        参数:
            lat: 目标点纬度（度）
            lon: 目标点经度（度）
            ref_lat: 参考点纬度（度）
            ref_lon: 参考点经度（度）
            radius: 地球半径（米），默认使用标准值
            
        返回:
            (x, y, azimuth): 
                x, y: 平面坐标（米）
                azimuth: 方位角（度，0-360，正北为0，顺时针增加）
        """
        # 角度转弧度
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)
        
        # 预计算三角函数值
        sin_ref_lat = math.sin(ref_lat_rad)
        cos_ref_lat = math.cos(ref_lat_rad)
        sin_lat = math.sin(lat_rad)
        cos_lat = math.cos(lat_rad)
        cos_d_lon = math.cos(lon_rad - ref_lon_rad)
        
        # 球面余弦定律计算角距离
        arg = sin_ref_lat * sin_lat + cos_ref_lat * cos_lat * cos_d_lon
        arg = min(max(arg, -1.0), 1.0)  # 确保在[-1,1]范围内
        c = math.acos(arg)
        
        # 计算比例因子k，处理中心点奇异性
        k = 1.0
        if abs(c) > 1e-10:
            k = c / math.sin(c)
        
        # 计算平面坐标
        x = radius * k * (cos_ref_lat * sin_lat - sin_ref_lat * cos_lat * cos_d_lon)
        y = radius * k * cos_lat * math.sin(lon_rad - ref_lon_rad)
        
        # 计算方位角（弧度）
        azimuth_rad = 0.0
        if abs(c) < 1e-10:
            # 当两点几乎重合时，方位角无明确意义，返回0（正北方向）
            azimuth_rad = 0.0
        else:
            sin_azimuth = (cos_lat * math.sin(lon_rad - ref_lon_rad)) / math.sin(c)
            cos_azimuth = (cos_ref_lat * sin_lat - sin_ref_lat * cos_lat * cos_d_lon) / math.sin(c)
            azimuth_rad = math.atan2(sin_azimuth, cos_azimuth)
        
        # 将方位角转换为度并归一化到[0, 360)
        azimuth_deg = math.fmod(math.degrees(azimuth_rad), 360.0)
        if azimuth_deg < 0:
            azimuth_deg += 360.0
        
        return x, y, azimuth_deg

    @staticmethod
    def inverse_project(x, y, ref_lat, ref_lon, radius=RADIUS_OF_EARTH):
        """
        从平面坐标反算回地理坐标
        
        参数:
            x: 平面x坐标（米）
            y: 平面y坐标（米）
            ref_lat: 参考点纬度（度）
            ref_lon: 参考点经度（度）
            radius: 地球半径（米），默认使用标准值
            
        返回:
            (lat, lon): 目标点的纬度和经度（度）
        """
        # 参考点经纬度转弧度
        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)
        
        # 计算从参考点到目标点的距离（米）
        rho = math.hypot(x, y)
        
        # 防止除零错误
        if rho < 1e-10:
            # 距离为0，即参考点本身
            return ref_lat, ref_lon
        
        # 计算角距离（弧度）
        c = rho / radius
        
        # 预计算三角函数值
        sin_c = math.sin(c)
        cos_c = math.cos(c)
        sin_ref_lat = math.sin(ref_lat_rad)
        cos_ref_lat = math.cos(ref_lat_rad)
        
        # 计算目标点纬度
        lat_rad = math.asin(cos_c * sin_ref_lat + (x * sin_c * cos_ref_lat) / rho)
        
        # 计算目标点经度
        lon_rad = ref_lon_rad + math.atan2(
            y * sin_c,
            rho * cos_ref_lat * cos_c - x * sin_ref_lat * sin_c
        )
        
        # 将弧度转换为度
        lat = math.degrees(lat_rad)
        lon = math.degrees(lon_rad)
        
        # 确保经度在[-180, 180)范围内
        lon = math.fmod(lon + 540.0, 360.0) - 180.0
        
        return lat, lon

if __name__ == "__main__":
    # 示例用法
    # 参考点
    ref_lat = 39.9042  # 纬度
    ref_lon = 116.4074  # 经度
    radius = RADIUS_OF_EARTH  # 地球半径

    # 目标点
    lat = 39.9152  # 纬度
    lon = 116.3974  # 经度

    # 进行投影
    x, y, azimuth = AzimuthalProjection.project(lat, lon, ref_lat, ref_lon, radius)  # 投影
    print(f"平面坐标: ({x:.2f}米, {y:.2f}米)")  # 输出平面坐标
    print(f"方位角: {azimuth:.2f}度")  # 输出方位角

    # 进行反投影    
    lat_reverse, lon_reverse = AzimuthalProjection.inverse_project(x, y, ref_lat, ref_lon, radius)  # 反投影
    print(f"反投影坐标: ({lat_reverse:.6f}度, {lon_reverse:.6f}度)")  # 输出反投影坐标

    # 验证反投影结果
    x_reverse, y_reverse, _ = AzimuthalProjection.project(lat_reverse, lon_reverse, ref_lat, ref_lon, radius)  # 再次投影
    print(f"验证平面坐标: ({x_reverse:.2f}米, {y_reverse:.2f}米)")  # 输出验证平面坐标


